import serial
import serial.tools.list_ports
import threading
import time
import re
import numpy as np
from scipy.optimize import least_squares


class Tag:
    def __init__(self, port, base_distance=1.0):
        available_ports = Tag.list_available_ports()  # 调用类方法获取可用端口列表
        port_available = False
        for comport in available_ports:
            if port in list(comport):  # 检查传入的端口是否在可用端口列表中
                port_available = True
                break
        if not port_available:
            print(f"端口 {port} 不可用，请检查。")
            exit(1)  # 若端口不可用，打印提示信息并退出程序
        self.port = port
        self.ser = serial.Serial(port, baudrate=115200, timeout=1)
        self.distance_list = [-1] * 4
        self.running = True
        self.thread = threading.Thread(target=self.read_serial)
        self.thread.daemon = True
        self.thread.start()
        self.base_distance = base_distance
        # 新增滤波队列（存储最近5次有效测量值）
        self.distance_history = [[] for _ in range(4)]  # 每个基站独立的历史队列

    @classmethod
    def list_available_ports(cls):
        """
        类方法，用于列出所有可用的串口信息
        """
        available_ports = []
        available_ports = list(serial.tools.list_ports.comports())
        if len(available_ports) <= 0:
            print("无串口设备。")
        else:
            print("可用的串口设备如下：")
            for comport in available_ports:
                print(list(comport)[0], list(comport)[1])
        return available_ports

    def read_serial(self):
        while self.running:
            try:
                data = self.ser.readline().decode('utf-8').strip()
                if data:
                    match = re.search(r'\$DIST,(M(\d+)),S(\d+),([\d.]+)', data)
                    if match:
                        m_index = int(match.group(2)) - 1  # M1 -> 0, M2 -> 1, M3 -> 2
                        s_index = int(match.group(3)) - 1  # S1 -> 0, S2 -> 1, S3 -> 2, etc.
                        value = float(match.group(4))
                        # 新增有效性检查和滤波处理
                        if 0 <= m_index < 3 and 0 <= s_index < 255 and value > 0:
                            # 维护历史队列（最多5个值）
                            history = self.distance_history[m_index]
                            history.append(value)
                            if len(history) > 5:
                                history.pop(0)

                            # 使用中位数滤波（比平均值更抗突变）
                            filtered_value = np.median(history) if history else value
                            self.distance_list[m_index] = filtered_value
            except serial.SerialException:
                print("Serial port error, retrying...")
                time.sleep(1)
                try:
                    self.ser.close()
                    self.ser.open()
                except Exception as e:
                    print(f"Failed to reopen serial port: {e}")
            except Exception as e:
                print(f"Error reading serial data: {e}")

    def calculate_tag_coordinates(self, distance_list=None):
        if distance_list is None:
            distance_list = self.distance_list

        def distance_equations(vars, distance_list):
            x, y = vars
            points = [
                (0, 0),
                (self.base_distance, 0),
                (self.base_distance, self.base_distance),
                (0, self.base_distance)
            ]
            equations = []
            valid_count = 0
            for i in range(4):  # 始终处理4个基站
                dist = distance_list[i] if i < len(distance_list) else -1
                if dist > 0:
                    dx = x - points[i][0]
                    dy = y - points[i][1]
                    equations.append(np.sqrt(dx ** 2 + dy ** 2) - dist)
                    valid_count += 1
            # 当有效基站≥3时添加稳定性约束
            if valid_count >= 3:
                equations.append(0.1 * (x - self.base_distance / 2))
                equations.append(0.1 * (y - self.base_distance / 2))
            return equations

        # 添加卡尔曼滤波
        if not hasattr(self, 'kf'):
            self.kf = {'x': self.base_distance / 2, 'y': self.base_distance / 2, 'p': 1.0}

        result = least_squares(
            distance_equations,
            [self.kf['x'], self.kf['y']],  # 使用滤波结果作为初始值
            args=(distance_list[:3],),

            ftol=1e-7,
            max_nfev=200
        )

        # 简易卡尔曼更新
        self.kf['x'] = 0.8 * self.kf['x'] + 0.2 * result.x[0]
        self.kf['y'] = 0.8 * self.kf['y'] + 0.2 * result.x[1]

        return [self.kf['x'], self.kf['y']]

    def calculate_tag_coordinates_simple(self, distance_list=None):
        if distance_list is None:
            distance_list = self.distance_list

        # 基站坐标布局（正方形）
        points = [
            (0, 0),
            (self.base_distance, 0),
            (self.base_distance, self.base_distance),
            (0, self.base_distance)
        ]

        # 收集有效距离数据（距离 > 0）
        valid_data = []
        for i, dist in enumerate(distance_list):
            if dist > 0 and i < len(points):
                valid_data.append((points[i][0], points[i][1], dist))

        # 至少需要3个有效基站才能三角定位
        if len(valid_data) < 3:
            return None

        # 取前三个有效基站进行定位
        (x1, y1, d1), (x2, y2, d2), (x3, y3, d3) = valid_data[:3]

        # 三角定位核心计算（三边测量法）
        try:
            # 构建线性方程组
            A = 2 * (x2 - x1)
            B = 2 * (y2 - y1)
            C = d1 ** 2 - d2 ** 2 - x1 ** 2 + x2 ** 2 - y1 ** 2 + y2 ** 2

            D = 2 * (x3 - x2)
            E = 2 * (y3 - y2)
            F = d2 ** 2 - d3 ** 2 - x2 ** 2 + x3 ** 2 - y2 ** 2 + y3 ** 2

            # 解线性方程组
            denominator = A * E - B * D
            if abs(denominator) < 1e-6:  # 防止除零错误
                return None

            x = (C * E - B * F) / denominator
            y = (A * F - C * D) / denominator

            return [x, y]

        except Exception as e:
            print(f"定位计算错误: {e}")
            return None

    def get_distance(self):
        return self.distance_list

    def set_port(self, port):
        self.running = False
        self.thread.join()
        self.port = port
        self.ser = serial.Serial(port, baudrate=115200, timeout=1)
        self.running = True
        self.thread = threading.Thread(target=self.read_serial)
        self.thread.start()

    def stop_thread(self):
        self.running = False
        if self.thread.is_alive():
            self.thread.join()


# 示例使用
if __name__ == "__main__":
    tag = Tag('COM5', base_distance=4.8)
    # x=tag.calculate_tag_coordinates([2.02, 3.05, 2.69, -1])
    # print(x)
    time.sleep(1)  # 等待线程启动并读取一些数据
    if Tag.list_available_ports():
        #tag = Tag('COM3')  # 替换为实际的串口编号
        time.sleep(2)  # 等待线程启动并读取一些数据
        while True:
            print(tag.get_distance())
            print(tag.calculate_tag_coordinates_simple(tag.get_distance()))
            time.sleep(0.1)
